/*
*	File: Quaternion.cpp
*	Date: 12/06/2011
*	Author: Ivan Gao
*	Desc:
*/
#include "Quaternion.h"
//namespace SimpleAnimation
//{
SQuaternion::SQuaternion () : x (0.0f), y (0.0f), z (0.0f), w (1.0f)
{

}

SQuaternion::SQuaternion (float _x, float _y, float _z, float _w) : x (_x), y(_y), z(_z), w(_w)
{

}

SQuaternion::SQuaternion (const SQuaternion & rQua) : x (rQua.x), y (rQua.y), z (rQua.z), w (rQua.w)
{

}

SQuaternion::~SQuaternion ()
{

} 

float SQuaternion::operator [] (int nIdx)
{
	return v[nIdx];
}

float SQuaternion::operator [] (int nIdx) const
{
	return v[nIdx];
}

SQuaternion & SQuaternion::operator = (const SQuaternion & rQua)
{
	int nLen =sizeof (v);
	memcpy (v, rQua.v, sizeof(v));
	return *this;
}

SQuaternion & SQuaternion::operator += (const SQuaternion & rQua)
{
	x += rQua.x;
	y += rQua.y;
	z += rQua.z;
	w += rQua.w;
	return *this;
}

SQuaternion & SQuaternion::operator -= (const SQuaternion & rQua)
{
	x -= rQua.x;
	y -= rQua.y;
	z -= rQua.z;
	w -= rQua.w;
	return *this;
}

SQuaternion & SQuaternion::operator *= (const SQuaternion & rQua)
{
	float mx (x), my (y), mz (z), mw (w);
	float qx (rQua.x), qy (rQua.y), qz (rQua.z), qw (rQua.w);
	x = mw * qx + mx * qw + my * qz - mz * qy;
	y = mw * qy - mx * qz + my * qw + mz * qx;
	z = mw * qz + mx * qy - my * qx + mz * qw;
	w = mw * qw - mx * qx - my * qy - mz * qz;
	return *this;
}

SQuaternion & SQuaternion::operator *= (const SVector3 & v)
{
	float qx, qy, qz, qw;
	qx = x;
	qy = y;
	qz = z;
	qw = w;

	x = qw * v.x            + qy * v.z - qz * v.y;
	y = qw * v.y - qx * v.z            + qz * v.x;
	z = qw * v.z + qx * v.y - qy * v.x;
	w =          - qx * v.x - qy * v.y - qz * v.z;
	return *this;
}

SQuaternion & SQuaternion::operator *= (float scale)
{
	x *= scale;
	y *= scale;
	z *= scale;
	w *= scale;
	return *this;
}

SQuaternion & SQuaternion::operator /= (float scale)
{
	float invScale = 1.0f / scale;
	x *= invScale;
	y *= invScale;
	z *= invScale;
	w *= invScale;
	return *this;
}

bool SQuaternion::operator == (const SQuaternion & rQua) const
{
	return (x == rQua.x && y == rQua.y && z == rQua.z && w == rQua.w);
}

bool SQuaternion::operator != (const SQuaternion & rQua) const
{
	return (x != rQua.x || y != rQua.y || z != rQua.z || w != rQua.w);
}

SQuaternion & SQuaternion::SphericalInterpolation (float d, const SQuaternion & rQua)
{
	float norm;
	norm = x * rQua.x + y * rQua.y + z * rQua.z + w * rQua.w;
	bool bFlip = false;
	if (norm < 0.0f)
	{
		norm = -norm;
		bFlip = true;
	} // if
	float inv_d;
	if (1.0f - norm < SIMPLEANIMATION_EPSILON)
	{
		inv_d = 1.0f - d;
	}
	else
	{
		float theta;
		theta = (float)acos(norm);
		float s = (float) (1.0f / sin(theta));
		inv_d = (float)sin ((1.0f - d) * theta) * s;
		d = (float)sin (d * theta) * s;
	} // if

	if (bFlip)
	{
		d = -d;
	} // if

	x = inv_d * x + d * rQua.x;
	y = inv_d * y + d * rQua.y;
	z = inv_d * z + d * rQua.z;
	w = inv_d * w + d * rQua.w;
	return *this;
} // SphericalInterpolation

SQuaternion & SQuaternion::Conjugate()
{
	x = -x;
	y = -y;
	z = -z;
	return *this;
}
	
SQuaternion & SQuaternion::Invert()
{
	x = -x;
	y = -y;
	z = -z;
	float norm = x * x + y * y + z * z + w * w;
	if (norm != 0.0f)
	{
		float invNorm = 1.0f / norm;
		x *= invNorm;
		y *= invNorm;
		z *= invNorm;
		w *= invNorm;
	}
	return *this;
} // Invert

SQuaternion operator * (const SQuaternion & q, const SQuaternion & r)
{
	return SQuaternion(
		r.w * q.x + r.x * q.w + r.y * q.z - r.z * q.y,
		r.w * q.y - r.x * q.z + r.y * q.w + r.z * q.x,
		r.w * q.z + r.x * q.y - r.y * q.x + r.z * q.w,
		r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z
		);
}

SQuaternion CrossProduct (const SQuaternion & q, const SQuaternion & r)
{
	return SQuaternion(
		r.w * q.x + r.x * q.w + r.y * q.z - r.z * q.y,
		r.w * q.y - r.x * q.z + r.y * q.w + r.z * q.x,
		r.w * q.z + r.x * q.y - r.y * q.x + r.z * q.w,
		r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z
		);
} // CrossProduct

float DotProduct (const SQuaternion & q, const SQuaternion & r)
{
	float len = q.x * r.x + q.y * r.y + q.z * r.z + q.w * r.w;
	return len;
} // DotProduct